#!/usr/bin/python3

import numpy as np
from scipy.spatial.transform import Rotation


def ToTransformNormForm(v, euler, euler_seq="zyx"):
    """ General form Transform, note 'xyz' is at T[3, :3]

    Args:
        v ([type]): [description]
        euler ([type]): [description]
        euler_seq (str, optional): [description]. Defaults to "zyx".

    Returns:
        [type]: [description]
    """
    T = np.eye(4)
    T[:3, 3] = v
    T[:3, :3] = Rotation.from_euler(euler_seq, euler).as_matrix()
    return T


def DaeToStl(file_path):
    # this is wrong
    import trimesh
    from path import Path
    link = trimesh.exchange.dae.load_collada(file_path)
    mesh = trimesh.Trimesh(**link['geometry'])
    file_path = Path(file_path).replace('.dae', '.stl')
    trimesh.exchange.export.export_mesh(mesh, file_path)


if __name__ == "__main__":
    import os
    from path import Path
    file_dir = Path(
        "/home/xiaodaxia/Rvbust/Sources/RobotZen/models/UR5/Visual")
    files = os.listdir(file_dir)
    for f in files:
        if f.endswith('dae'):
            DaeToStl(file_dir / f)
            print(f"Converting {f}")
